#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "pcl_conversions/pcl_conversions.h"
#include "pcl/point_types.h"
#include "pcl/PCLPointCloud2.h"
#include "pcl/conversions.h"
#include <pcl/io/pcd_io.h>
#include <cmath>
#include <CL/cl.h>
#include "cl_process.hpp"
#include "merged_cloud.hpp"
#include <pcl/filters/statistical_outlier_removal.h>
///imu convert
#include "sensor_msgs/msg/imu.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2_eigen/tf2_eigen.hpp"
#include <Eigen/Dense>
#include "filter.hpp"
//convet end
#define imu_sw 0
//#define max_frames 6
// 重新定义节点类型
struct PointXYZIntensities
{
    float x;
    float y;
    float z;
    float intensities; 
};

POINT_CLOUD_REGISTER_POINT_STRUCT(
    PointXYZIntensities,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensities, intensities)
);

class PointCloudTransformerNode : public rclcpp::Node
{
public:
    PointCloudTransformerNode() : Node("pointcloud_transformer_node")
    {
        // 声明参数并从YAML文件加载
        this->declare_parameter<double>("rotation.alpha", 0.0);
        this->declare_parameter<double>("rotation.beta", 0.0);
        this->declare_parameter<double>("rotation.omega", 0.0);
        this->declare_parameter<double>("translation.x0", 0.0);
        this->declare_parameter<double>("translation.y0", 0.0);
        this->declare_parameter<double>("translation.z0", 0.0);
        this->declare_parameter<double>("max_x", 0.0);
        this->declare_parameter<double>("min_x", 0.0);
        this->declare_parameter<double>("max_y", 0.0);
        this->declare_parameter<double>("min_y", 0.0);
        this->declare_parameter<double>("max_z", 0.0);
        this->declare_parameter<double>("min_z", 0.0);
        this->declare_parameter("sor.mean_k", 50);
        this->declare_parameter("sor.std_mul", 1.0);
        this->declare_parameter<int>("max_frames",5);
        this->declare_parameter<int>("filter_mode",0); //滤波模式 0-小目标 1-中目标 2-大目标（井盖）
        ///小放射源滤波
        this->declare_parameter<float>("filter_small_0.left_up_y", 0.1);
        this->declare_parameter<float>("filter_small_0.left_up_z", 0.1);
        this->declare_parameter<float>("filter_small_0.right_down_y", 0.1);
        this->declare_parameter<float>("filter_small_0.right_down_z", 0.1);
        //中放射源滤波
        this->declare_parameter<float>("filter_small_1.left_up_y", 0.1);
        this->declare_parameter<float>("filter_small_1.left_up_z", 0.1);
        this->declare_parameter<float>("filter_small_1.right_down_y", 0.1);
        this->declare_parameter<float>("filter_small_1.right_down_z", 0.1);
        this->declare_parameter<float>("filter_small_2.left_up_y", 0.1);
        this->declare_parameter<float>("filter_small_2.left_up_z", 0.1);
        this->declare_parameter<float>("filter_small_2.right_down_y", 0.1);
        this->declare_parameter<float>("filter_small_2.right_down_z", 0.1);
        ///imu convert
         // 声明并获取旋转角度参数（单位：度）
        this->declare_parameter<double>("imu.angle_x", 0.0);
        this->declare_parameter<double>("imu.angle_y", 0.0);
        this->declare_parameter<double>("imu.angle_z", 0.0);
         double angle_x, angle_y, angle_z;
        this->get_parameter("angle_x", angle_x);
        this->get_parameter("angle_y", angle_y);
        this->get_parameter("angle_z", angle_z);
        // 将角度转换为弧度
        angle_x_rad_ = angle_x * M_PI / 180.0;
        angle_y_rad_ = angle_y * M_PI / 180.0;
        angle_z_rad_ = angle_z * M_PI / 180.0;
        // 预计算旋转矩阵
        calculateRotationMatrix();
        // 创建订阅者
        if(imu_sw==1)
        {
            imu_subscriber_ = this->create_subscription<sensor_msgs::msg::Imu>(
                "/lidar_imu",
                10,
                std::bind(&PointCloudTransformerNode::imuCallback, this, std::placeholders::_1)
            );
         }   
        // 创建发布者
        imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>(
            "/lidar_imu/calibrated",
            10
        );
        //imu end
        // 订阅原始点云
        subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "pointcloud", 10, 
            std::bind(&PointCloudTransformerNode::cloud_callback, this, std::placeholders::_1)
        );
        
        // 发布变换后的点云
        publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("transformed_cloud", 10);
        //发布过滤后的点云
        filter_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("filtered_cloud", 10);
        // 初始化OpenCL
        init_opencl();
        
        RCLCPP_INFO(this->get_logger(), "PointCloud Transformer Node initialized");
         alpha = this->get_parameter("rotation.alpha").as_double();
         beta = this->get_parameter("rotation.beta").as_double();
         omega = this->get_parameter("rotation.omega").as_double();
         x0 = this->get_parameter("translation.x0").as_double();
         y0 = this->get_parameter("translation.y0").as_double();
         z0 = this->get_parameter("translation.z0").as_double();
         max_x= this->get_parameter("max_x").as_double();
         min_x= this->get_parameter("min_x").as_double();
         max_y= this->get_parameter("max_y").as_double();
         min_y= this->get_parameter("min_y").as_double();
         max_z= this->get_parameter("max_z").as_double();
         min_z= this->get_parameter("min_z").as_double();
         this->get_parameter("sor.mean_k", sor_mean_k_);
         this->get_parameter("sor.std_mul", sor_std_mul_);
         max_frames= this->get_parameter("max_frames").as_int();

         this->get_parameter("filter_mode", fangshe_filter_.filter_mode);
         this->get_parameter("filter_small_0.left_up_y", fangshe_filter_.filter_small_0.left_up_y);
         this->get_parameter("filter_small_0.left_up_z", fangshe_filter_.filter_small_0.left_up_z);
         this->get_parameter("filter_small_0.right_down_y", fangshe_filter_.filter_small_0.right_down_y);
         this->get_parameter("filter_small_0.right_down_z", fangshe_filter_.filter_small_0.right_down_z);
         this->get_parameter("filter_small_1.left_up_y", fangshe_filter_.filter_small_1.left_up_y);
         this->get_parameter("filter_small_1.left_up_z", fangshe_filter_.filter_small_1.left_up_z);
         this->get_parameter("filter_small_1.right_down_y", fangshe_filter_.filter_small_1.right_down_y);
         this->get_parameter("filter_small_1.right_down_z", fangshe_filter_.filter_small_1.right_down_z);
         this->get_parameter("filter_small_2.left_up_y", fangshe_filter_.filter_small_2.left_up_y);
         this->get_parameter("filter_small_2.left_up_z", fangshe_filter_.filter_small_2.left_up_z);
         this->get_parameter("filter_small_2.right_down_y", fangshe_filter_.filter_small_2.right_down_y);
         this->get_parameter("filter_small_2.right_down_z", fangshe_filter_.filter_small_2.right_down_z);

          // 将角度转换为弧度
        alpha = alpha * M_PI / 180.0;
        beta = beta * M_PI / 180.0;
        omega = omega * M_PI / 180.0;
        calculate_rotation_matrix(alpha, beta, omega, rotation);
        cloud_merger.set_max_frames(max_frames);

        // RCLCPP_INFO(this->get_logger(),"max_x=%5.2f",max_x);
    }
    
    ~PointCloudTransformerNode()
    {
        // 释放OpenCL资源
        release_opencl();
       }

private:
    int filter_xyz(pcl::PointXYZI p);    
    void removeOutliers(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud);

    void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        // 从参数服务器获取最新参数
        //RCLCPP_INFO(this->get_logger(),"start gpu!");
       
        // RCLCPP_INFO(this->get_logger(),"Received point cloud");
     
        
        // 计算旋转矩阵 (Z-Y-X 欧拉角)
        //float rotation[9];
       // calculate_rotation_matrix(alpha, beta, omega, rotation);
        
        // 转换为PCL点云
       // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr laser_cloud(new pcl::PointCloud<pcl::PointXYZI>);
            //自定义点类型解析
          //  pcl::PointCloud<PointXYZIntensities>::Ptr custom_cloud(new pcl::PointCloud<PointXYZIntensities>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr custom_cloud(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::fromROSMsg(*msg, *custom_cloud);
        std::size_t size=custom_cloud->size();
        if (custom_cloud->empty()) {
            RCLCPP_WARN(this->get_logger(), "Received empty point cloud");
            return;
        }
       // else RCLCPP_INFO(this->get_logger(),"receive point=%d",size);
        
     
    
        // 使用OpenCL进行点云变换
        RCLCPP_INFO(this->get_logger(),"start gpu!");
        transform_pointcloud(custom_cloud, rotation, x0, y0, z0);
         RCLCPP_INFO(this->get_logger(),"end gpu!=%d",custom_cloud->size());

        custom_cloud->width=custom_cloud->size();
        custom_cloud->height=1;

        //  std::string filename = "transform_pointcloud.pcd";
        //         pcl::io::savePCDFileASCII(filename, *custom_cloud);
        //过滤天花板
        size_t cloud_sum=0;
  
        for(int i=0;i<custom_cloud->width;i++) 
            if(filter_xyz(custom_cloud->points[i])!=0)
                cloud_sum++;

        laser_cloud->resize(cloud_sum);
        laser_cloud->width= cloud_sum;
        laser_cloud->height=1;
        int jj=0;
        for(int i=0;i<custom_cloud->width;i++)
        {
        //   if((custom_cloud->points[i].z>0.1)&&(custom_cloud->points[i].z<1.0)&&(custom_cloud->points[i].y<1.2)&&
        //         (custom_cloud->points[i].y>-1.3)&&(custom_cloud->points[i].x<-0.1))  //点数条件
           if(filter_xyz(custom_cloud->points[i])!=0)
          {  
          laser_cloud->points[jj] =  custom_cloud->points[i];
          jj++;
          }
        }

        //////////拼接点云//////////////////////////////////
        // ... 填充点云数据 ...
        cloud_merger.add_cloud(laser_cloud);
       // RCLCPP_INFO(this->get_logger(),"merged!=%d psum=%d",cloud_merger.get_fram_sum(),cloud_sum);
        //////////离群点过滤//////////
       auto merged_cloud = cloud_merger.get_merged_cloud();
       removeOutliers(merged_cloud);
       
        ///////////////////////////
        // 转换回ROS消息并发布
        if(cloud_merger.get_fram_sum()==max_frames)
        {
        pcl::PointCloud<pcl::PointXYZI>::Ptr merged_c=cloud_merger.get_merged_cloud();
        sensor_msgs::msg::PointCloud2 output_msg;
        pcl::toROSMsg(*merged_c, output_msg);
       // RCLCPP_INFO(this->get_logger(),"merged!=%d",merged_c->size());
        output_msg.header = msg->header;  // 保持相同的坐标系和时间戳
        output_msg.header.frame_id  = "map";
        publisher_->publish(output_msg);
        //RCLCPP_INFO(this->get_logger(),"public cloud!=%d",merged_c->size());
        //过滤点云发布
        // fangshe_filter_;
        // fangshe_filter_.setInputCloud(merged_c);
        // fangshe_filter_.filterCloud(1);
        // fangshe_filter_.voxelFilter();
        // pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud = fangshe_filter_.getFilteredCloud();
        // sensor_msgs::msg::PointCloud2 filtered_msg;
        // pcl::toROSMsg(*filtered_cloud, filtered_msg);
        // filtered_msg.header = msg->header;
        // filtered_msg.header.frame_id = "map";
        // filter_publisher_->publish(filtered_msg);
        // RCLCPP_INFO(this->get_logger(),"public filter cloud!=%d",filtered_cloud->size());

        }
    }
    //小放射源滤波参数
    PointCloudFilter fangshe_filter_;
    // ROS相关成员
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filter_publisher_;
    float alpha,beta,omega,x0,y0,z0;
    float rotation[9];
    // 创建拼接实例
    Merged_cloud cloud_merger;
    float min_x,min_y,min_z;
    float max_x,max_y,max_z;
    int sor_mean_k_;
    double sor_std_mul_;
    int max_frames;
    ///imu convert ///////////////////////////
    ///////////////////////////////////////////
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
    double angle_x_rad_, angle_y_rad_, angle_z_rad_;
    Eigen::Quaterniond rotation_matrix_;
/////////////////////////////////////////////////////
    void calculateRotationMatrix()
    {
        // // 创建三个轴的旋转矩阵
        // Eigen::AngleAxisd rollAngle(angle_x_rad_, Eigen::Vector3d::UnitX());
        // Eigen::AngleAxisd pitchAngle(angle_y_rad_, Eigen::Vector3d::UnitY());
        // Eigen::AngleAxisd yawAngle(angle_z_rad_, Eigen::Vector3d::UnitZ());
        // 计算绕各轴的旋转矩阵
        double cos_x = cos(angle_x_rad_), sin_x = sin(angle_x_rad_);
        double cos_y = cos(angle_y_rad_), sin_y = sin(angle_y_rad_);
        double cos_z = cos(angle_z_rad_), sin_z = sin(angle_z_rad_);
        // // 组合旋转矩阵 (Z * Y * X)
        // rotation_matrix_ = yawAngle * pitchAngle * rollAngle;
        // 绕X轴旋转矩阵
        Eigen::Matrix3d Rx;
        Rx << 1, 0, 0,
              0, cos_x, -sin_x,
              0, sin_x, cos_x;
        
        // 绕Y轴旋转矩阵
        Eigen::Matrix3d Ry;
        Ry << cos_y, 0, sin_y,
              0, 1, 0,
              -sin_y, 0, cos_y;
        
        // 绕Z轴旋转矩阵
        Eigen::Matrix3d Rz;
        Rz << cos_z, -sin_z, 0,
              sin_z, cos_z, 0,
              0, 0, 1;
        rotation_matrix_ = Rz * Ry * Rx;
    }
    
    void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        // 转换原始四元数到Eigen
        Eigen::Quaterniond original_quat(
            msg->orientation.w,
            msg->orientation.x,
            msg->orientation.y,
            msg->orientation.z
        );
        
        // 应用旋转校正
        Eigen::Quaterniond corrected_quat = rotation_matrix_ * original_quat;
        corrected_quat.normalize();
        
        // 校正角速度
        Eigen::Vector3d angular_vel(msg->angular_velocity.x, 
                                   msg->angular_velocity.y, 
                                   msg->angular_velocity.z);
        Eigen::Vector3d corrected_angular_vel = rotation_matrix_ * angular_vel;
        
        // 校正线加速度
        Eigen::Vector3d linear_accel(msg->linear_acceleration.x, 
                                    msg->linear_acceleration.y, 
                                    msg->linear_acceleration.z);
        Eigen::Vector3d corrected_linear_accel = rotation_matrix_ * linear_accel;
        
        // 创建校正后的消息
        auto calibrated_msg = std::make_unique<sensor_msgs::msg::Imu>();
        calibrated_msg->header = msg->header;
        calibrated_msg->header.frame_id = "lidar_imu_calibrated";  // 可选：更新坐标系
        
        // 设置校正后的 orientation
        calibrated_msg->orientation.x = corrected_quat.x();
        calibrated_msg->orientation.y = corrected_quat.y();
        calibrated_msg->orientation.z = corrected_quat.z();
        calibrated_msg->orientation.w = corrected_quat.w();
        
        // 设置校正后的 angular velocity
        calibrated_msg->angular_velocity.x = corrected_angular_vel.x();
        calibrated_msg->angular_velocity.y = corrected_angular_vel.y();
        calibrated_msg->angular_velocity.z = corrected_angular_vel.z();
        
        // 设置校正后的 linear acceleration
        calibrated_msg->linear_acceleration.x = corrected_linear_accel.x();
        calibrated_msg->linear_acceleration.y = corrected_linear_accel.y();
        calibrated_msg->linear_acceleration.z = corrected_linear_accel.z();
        
        //  covariance 矩阵也需要相应变换，这里简化处理，直接复制
        calibrated_msg->orientation_covariance = msg->orientation_covariance;
        calibrated_msg->angular_velocity_covariance = msg->angular_velocity_covariance;
        calibrated_msg->linear_acceleration_covariance = msg->linear_acceleration_covariance;
        
        // 发布校正后的消息
        imu_publisher_->publish(std::move(calibrated_msg));
    }
    ///imu convert end
};
int PointCloudTransformerNode::filter_xyz(pcl::PointXYZI p)
{
  int flag=0;
    if((p.z>min_z)&&(p.z<max_z)&&(p.y<max_y)&&(p.y>min_y)&&(p.x>min_x)&&(p.x<max_x))
        {
            flag=1;
            if(fangshe_filter_.filter_mode==0)
            {
                flag=0;
             if((p.z<fangshe_filter_.filter_small_0.left_up_z)&&(p.z>fangshe_filter_.filter_small_0.right_down_z)&&
                (p.y<fangshe_filter_.filter_small_0.left_up_y)&&(p.y>fangshe_filter_.filter_small_0.right_down_y))
                flag=1;
            }
        }
       // flag=1;
    return flag;
}
void PointCloudTransformerNode::removeOutliers(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)
{
    if (cloud->empty()) {
        return;
    }
    
    pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
    sor.setInputCloud(cloud);
    sor.setMeanK(sor_mean_k_);
    sor.setStddevMulThresh(sor_std_mul_);
    sor.filter(*cloud);
}

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PointCloudTransformerNode>());
    rclcpp::shutdown();
    return 0;
}
    